package com.example.originbot;

import com.google.protobuf.ByteString;
import com.google.protobuf.InvalidProtocolBufferException;

import ros2.proto.LaserScanOuterClass;
import ros2.proto.OdometryOuterClass;

/**
 * ROS Protobuf协议
 * */
public class ROSProtobuf {
    private long preSendTime = 0;
    private TCPClient client;

    public ROSProtobuf(TCPClient client) {
        this.client = client;
    }

    void sendTwist(double linear, double angular) {
        if (client == null) {
            return;
        }
        if (linear != 0 && angular != 0) {
            // 限制发送最小间隔 100ms
            if (System.currentTimeMillis() - preSendTime < 100) {
                return;
            }
        }
        preSendTime = System.currentTimeMillis();
        OdometryOuterClass.geometry_msgs.Vector3.Builder linear_b = OdometryOuterClass.geometry_msgs.Vector3.newBuilder();
        OdometryOuterClass.geometry_msgs.Vector3.Builder angular_b = OdometryOuterClass.geometry_msgs.Vector3.newBuilder();
        OdometryOuterClass.geometry_msgs.Twist.Builder twist = OdometryOuterClass.geometry_msgs.Twist.newBuilder();
        linear_b.setX(linear);
        angular_b.setZ(angular);
        twist.setLinear(linear_b.build());
        twist.setAngular(angular_b.build());
        Ros.ROS2.Builder ros2 = Ros.ROS2.newBuilder();
        ros2.setMsgType(Ros.ROS2.MsgType.CMD_VEL_PUBLISH);
        ros2.setMsgData(twist.build().toByteString());
        ByteString bs = ros2.build().toByteString();
        ByteString bs2 = ByteString.copyFrom(TCPClient.kDelimiter);
        bs = bs.concat(bs2);
        client.sendData(bs);
    }

    boolean sendAuth(String user, String pass)
    {
        if (client == null) {
            return false;
        }
        DeviceAuthOuterClass.DeviceAuth.Builder auth = DeviceAuthOuterClass.DeviceAuth.newBuilder();
        auth.setUserName(user);
        auth.setUserPasswd(pass);
        Ros.ROS2.Builder ros2 = Ros.ROS2.newBuilder();
        ros2.setMsgType(Ros.ROS2.MsgType.DEVICE_AUTH);
        ros2.setMsgData(auth.build().toByteString());
        ByteString bs = ros2.build().toByteString();
        ByteString bs2 = ByteString.copyFrom(TCPClient.kDelimiter);
        bs = bs.concat(bs2);
        return client.sendData(bs.toByteArray());
    }

    static class ReceivedInterface {
        void receivedH26XFrame(H26XFrameOuterClass.H26XFrame h26x){}
        void receivedOdometry(OdometryOuterClass.Odometry odom){}
        void receivedOriginBotStatus(OriginBotStatusOuterClass.OriginBotStatus status){}
        void receivedDeviceAuth(DeviceAuthOuterClass.DeviceAuth auth){}
        void receivedLaserScan(LaserScanOuterClass.LaserScan laser){}
    }

    /**
     * 解析protobuf数据
     * */
    boolean parseFromBytes(byte[] packet, ReceivedInterface receivedInterface)
    {
        try {
            Ros.ROS2 ros2 = Ros.ROS2.parseFrom(packet);
            Ros.ROS2.MsgType type = ros2.getMsgType();
            switch (type) {
                case H26X_FRAME:
                    H26XFrameOuterClass.H26XFrame h26x = H26XFrameOuterClass.H26XFrame.parseFrom(ros2.getMsgData());
                    if (receivedInterface != null)
                        receivedInterface.receivedH26XFrame(h26x);
                    break;
                case ODO_METRY:
                    OdometryOuterClass.Odometry odom = OdometryOuterClass.Odometry.parseFrom(ros2.getMsgData());
                    if (receivedInterface != null)
                        receivedInterface.receivedOdometry(odom);
                    break;
                case ORIGIN_BOT_STATUS:
                    OriginBotStatusOuterClass.OriginBotStatus status = OriginBotStatusOuterClass.OriginBotStatus.parseFrom(ros2.getMsgData());
                    if (receivedInterface != null)
                        receivedInterface.receivedOriginBotStatus(status);
                    break;
                case DEVICE_AUTH:
                    DeviceAuthOuterClass.DeviceAuth auth = DeviceAuthOuterClass.DeviceAuth.parseFrom(ros2.getMsgData());
                    if (receivedInterface != null)
                        receivedInterface.receivedDeviceAuth(auth);
                    break;
                case LASER_SCAN:
                    LaserScanOuterClass.LaserScan laser = LaserScanOuterClass.LaserScan
                            .parseFrom(ros2.getMsgData());
                    if (receivedInterface != null)
                        receivedInterface.receivedLaserScan(laser);
                    break;
                default:
                    break;
            }
            return true;
        } catch (InvalidProtocolBufferException e) {
            System.out.println("InvalidProtocolBufferException:" + e.getMessage());
        }
        return false;
    }
}
